#include "rclcpp/rclcpp.hpp"
#include "robot_control/srv/robot_state_srv.hpp"
#include "std_srvs/srv/empty.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rcl_interfaces/srv/get_parameters.hpp"

#include <chrono>
#include <thread>

using namespace std::chrono_literals;

class StateService : public rclcpp::Node
{
public:
    StateService() : Node("robot_service")
    {
        // 声明参数
        this->declare_parameter("max_linear_speed", 0.2);
        this->declare_parameter("lidar_publish_frequency", 10.0);
        this->declare_parameter("obstacle_threshold", 0.5);
        
        // 初始化机器人状态
        robot_state_ = "idle";
        battery_level_ = 100.0;
        
        // 创建状态查询服务
        get_state_service_ = this->create_service<robot_control::srv::RobotStateSrv>(
            "/get_robot_state",
            std::bind(&StateService::handle_get_state, this,
                     std::placeholders::_1, std::placeholders::_2));
        
        // 创建重置服务
        reset_service_ = this->create_service<std_srvs::srv::Empty>(
            "/reset_robot", 
            std::bind(&StateService::handle_reset, this,
                     std::placeholders::_1, std::placeholders::_2));
        
        // 创建参数变更回调
        param_callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&StateService::param_callback, this, std::placeholders::_1));
        
        // 创建电池消耗定时器（模拟电池消耗）
        battery_timer_ = this->create_wall_timer(
            10s, std::bind(&StateService::battery_timer_callback, this));
            
        RCLCPP_INFO(this->get_logger(), "机器人状态服务已启动");
        RCLCPP_INFO(this->get_logger(), "初始状态: %s, 电量: %.1f%%", 
                   robot_state_.c_str(), battery_level_);
    }

private:
    void handle_get_state(
        const std::shared_ptr<robot_control::srv::RobotStateSrv::Request> request,
        const std::shared_ptr<robot_control::srv::RobotStateSrv::Response> response)
    {
        (void)request;  // 请求为空，忽略未使用警告
        
        // 模拟查询状态消耗电量
        if (battery_level_ > 0.1) {
            battery_level_ -= 0.1;
        }
        
        response->state = robot_state_;
        response->battery = battery_level_;
        
        RCLCPP_INFO(this->get_logger(), "状态查询: %s, 电量: %.1f%%", 
                   robot_state_.c_str(), battery_level_);
    }
    
    void handle_reset(
        const std::shared_ptr<std_srvs::srv::Empty::Request> request,
        const std::shared_ptr<std_srvs::srv::Empty::Response> response)
    {
        (void)request;  // 忽略请求
        (void)response; // 忽略响应
        
        robot_state_ = "idle";
        battery_level_ = 100.0;
        
        RCLCPP_INFO(this->get_logger(), "机器人已重置: 状态=%s, 电量=%.1f%%", 
                   robot_state_.c_str(), battery_level_);
    }
    
    rcl_interfaces::msg::SetParametersResult param_callback(
        const std::vector<rclcpp::Parameter> &parameters)
    {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        
        for (const auto &param : parameters) {
            if (param.get_name() == "max_linear_speed") {
                double new_speed = param.as_double();
                RCLCPP_INFO(this->get_logger(), "参数已更新: max_linear_speed = %.2f m/s", new_speed);
            }
            else if (param.get_name() == "lidar_publish_frequency") {
                double new_freq = param.as_double();
                RCLCPP_INFO(this->get_logger(), "参数已更新: lidar_publish_frequency = %.1f Hz", new_freq);
            }
            else if (param.get_name() == "obstacle_threshold") {
                double new_threshold = param.as_double();
                RCLCPP_INFO(this->get_logger(), "参数已更新: obstacle_threshold = %.2f m", new_threshold);
            }
        }
        
        return result;
    }
    
    void battery_timer_callback()
    {
        // 如果机器人在移动状态，消耗更多电量
        if (robot_state_ == "moving") {
            battery_level_ -= 2.0;
        } else {
            battery_level_ -= 0.5;
        }
        
        // 电量保护
        if (battery_level_ < 0) {
            battery_level_ = 0;
            robot_state_ = "error";
            RCLCPP_WARN(this->get_logger(), "电量耗尽！机器人进入错误状态");
        }
        else if (battery_level_ < 20.0) {
            RCLCPP_WARN(this->get_logger(), "电量不足: %.1f%%，请及时充电", battery_level_);
        }
    }
    
    rclcpp::Service<robot_control::srv::RobotStateSrv>::SharedPtr get_state_service_;
    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr reset_service_;
    rclcpp::TimerBase::SharedPtr battery_timer_;
    OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
    
    std::string robot_state_;
    double battery_level_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<StateService>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}